//
// Created by 马皓然 on 2025/10/25.
//
#include "motion_planner.h"
#include <math.h>

/**
 * @brief  初始化规划器结构体
 * @param  planner: 指向规划器结构体的指针
 * @retval None
 */
void Planner_Init(Trapezoid_Planner_t *planner) {
    planner->start_pos = 0.0f;
    planner->target_pos = 0.0f;
    planner->max_vel = 0.0f;
    planner->accel = 0.0f;
    planner->current_time = 0.0f;
    planner->total_time = 0.0f;
    planner->pos_output = 0.0f;
    planner->vel_output = 0.0f;
    planner->is_planning = false;
}

/**
 * @brief  启动一次新的梯形运动规划
 * @param  planner: 规划器结构体指针
 * @param  current_motor_pos: 电机当前的实际位置（作为运动起点）
 * @param  target_pos: 目标位置
 * @param  max_vel: 最大运行速度 (必须是正值)
 * @param  accel: 加速度 (必须是正值)
 * @retval None
 */
void Planner_Start_Motion(Trapezoid_Planner_t *planner,
                          float target_pos,
                          float max_vel,
                          float accel) {
    if (accel <= 0 || max_vel <= 0) {
        return; // 参数无效
    }

    planner->start_pos = 0.0f;
    planner->target_pos = target_pos;
    planner->max_vel = max_vel;
    planner->accel = accel;
    planner->current_time = 0.0f;
    planner->is_planning = true;

    float distance = target_pos - 0.0f;
    float abs_dist = fabsf(distance);

    // 1. 计算加速时间 Ta
    float Ta = planner->max_vel / planner->accel;
    // 2. 计算加速距离 Da
    float Da = 0.5f * planner->accel * Ta * Ta;

    // 3. 判断是否能达到最大速度 (三角/梯形)
    if (abs_dist > 2.0f * Da) {
        // 梯形规划 (加速-匀速-减速)
        float Dv = abs_dist - 2.0f * Da;  // 匀速距离
        float Tv = Dv / planner->max_vel; // 匀速时间
        planner->total_time = 2.0f * Ta + Tv;
    } else {
        // 三角规划 (全程都在加速/减速)
        Ta = sqrtf(abs_dist / planner->accel); // 新的加速时间
        planner->total_time = 2.0f * Ta;
        Da = abs_dist / 2.0f; // 更新加速距离
    }

    // 将计算结果存入结构体，供后续 Update 使用
}

/**
 * @brief  在每个控制周期更新规划器状态，生成新的目标位置和速度
 * @param  planner: 规划器结构体指针
 * @param  cycle_time_s: 当前周期的时间长度 (单位：秒, 如 0.001s for 1ms)
 * @retval None
 * @note   此函数应在固定频率的定时器中断中调用。
 */
void Planner_Update(Trapezoid_Planner_t *planner, float cycle_time_s) {
    if (!planner->is_planning) {
        return;
    }

    planner->current_time += cycle_time_s;
    float t = planner->current_time;
    float pos_output = 0.0f;
    float vel_output = 0.0f;

    float distance = planner->target_pos - planner->start_pos;
    float direction = (distance >= 0) ? 1.0f : -1.0f;
    float abs_dist = fabsf(distance);

    // 计算加速时间 Ta (取决于是否是三角规划)
    float Ta;
    if (planner->total_time * planner->accel <= 2.0f * planner->max_vel) {
        // 三角形：Ta = T_total / 2
        Ta = planner->total_time / 2.0f;
    } else {
        // 梯形：Ta = V_max / A_max
        Ta = planner->max_vel / planner->accel;
    }

    // 运动已经结束
    if (t >= planner->total_time) {
        planner->is_planning = false;
        t = planner->total_time; // 确保输出停留在终点
        vel_output = 0.0f;
        pos_output = planner->target_pos;
    }
    // 1. 加速段 (0 < t <= Ta)
    else if (t <= Ta) {
        vel_output = planner->accel * t;
        pos_output = 0.5f * planner->accel * t * t;
    }
    // 3. 减速段 (T_total - Ta < t < T_total)
    else if (t > (planner->total_time - Ta)) {
        float T_dec = planner->total_time - t; // 减速时间
        // 使用减速时间计算速度和位移，并减去总位移
        float Da = 0.5f * planner->accel * Ta * Ta; // 加速距离
        float Dv = abs_dist - 2.0f * Da;            // 匀速距离 (如果 Tv>0)
        float V_max = planner->accel * Ta;          // 实际达到的最大速度
        float Pos_total_at_start_dec;

        if (Da * 2.0f >= abs_dist) {
            // 三角形：减速起点在总距离一半
            Pos_total_at_start_dec = abs_dist - 0.5f * planner->accel * Ta * Ta;
        } else {
            // 梯形：减速起点在加速段和匀速段之后
            Pos_total_at_start_dec = Da + Dv;
        }

        vel_output = planner->accel * T_dec; // 减速段速度变化
        pos_output = Pos_total_at_start_dec + (V_max * T_dec - 0.5f * planner->accel * T_dec * T_dec);
    }
    // 2. 匀速段 (Ta < t <= T_total - Ta)
    else {
        float V_max = planner->accel * Ta;
        float Da = 0.5f * planner->accel * Ta * Ta;
        float Tv = planner->total_time - 2.0f * Ta; // 匀速时间
        float T_vel = t - Ta; // 匀速段运行时间

        vel_output = V_max;
        pos_output = Da + V_max * T_vel;
    }

    // 加上起始位置和方向
    planner->vel_output = vel_output * direction;
    planner->pos_output = planner->start_pos + pos_output * direction;
}

/**
 * @brief  将规划器的输出值应用于电机管理层
 * @param  planner: 规划器结构体指针
 * @param  motor: 电机实例结构体指针
 * @retval None
 */
void Planner_Apply_To_Motor(const Trapezoid_Planner_t *planner, Motor_Instance_t *motor) {
    if (motor->state == MOTOR_STATE_RUNNING || motor->state == MOTOR_STATE_ENABLED) {
        // 更新电机目标值
        motor->target_pos = planner->pos_output;
        motor->target_vel = planner->vel_output;
    }
}